from maix import camera, display, app, time, image, pinmap, uart
import cv2
import numpy as np
import struct

device = "/dev/ttyS0"
serial0 = uart.UART(device, 115200)

cam = camera.Camera(320, 240)
disp = display.Display()

frame_count = 0
process_every_n_frames = 2  # 每2帧处理一次
show_perspective = False  # 切换显示模式：True显示透视变换，False显示原图+标记

# 显示屏幕尺寸
screen_width = 320
screen_height = 240

# 透视变换后图像的中心点坐标
perspective_center_x = screen_width // 2   # 160
perspective_center_y = screen_height // 2  # 120

# 十字的坐标和参数（x 由公式动态计算，y 固定）
cross_x = screen_width // 2   # 占位，会在检测后更新
cross_y = 106
cross_size = 10
cross_color_green = (0, 255, 0)  # 绿色
cross_thickness = 2



def order_points(pts):
    """按照左上、右上、右下、左下的顺序排列角点"""
    rect = np.zeros((4, 2), dtype="float32")
    s = pts.sum(axis=1)
    rect[0] = pts[np.argmin(s)]  # 左上
    rect[2] = pts[np.argmax(s)]  # 右下
    diff = np.diff(pts, axis=1)
    rect[1] = pts[np.argmin(diff)]  # 右上
    rect[3] = pts[np.argmax(diff)]  # 左下
    return rect

def find_rectangles(gray_img):
    """检测矩形并返回角点、中心点、面积、周长"""
    rectangles = []
    blurred = cv2.GaussianBlur(gray_img, (5, 5), 0)
    edged = cv2.Canny(blurred, 60, 180)
    contours, _ = cv2.findContours(edged, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    for contour in contours:
        perimeter = cv2.arcLength(contour, True)
        epsilon = 0.02 * perimeter
        approx = cv2.approxPolyDP(contour, epsilon, True)
        if len(approx) == 4:
            area = cv2.contourArea(contour)
            if area > 500:
                corners = approx.reshape(4, 2)
                ordered_corners = order_points(corners.astype("float32"))
                center_x = int(np.mean(corners[:, 0]))
                center_y = int(np.mean(corners[:, 1]))
                rectangles.append({
                    'corners': ordered_corners,
                    'center': (center_x, center_y),
                    'area': area,
                    'perimeter': perimeter
                })

    rectangles.sort(key=lambda x: x['area'], reverse=True)
    return rectangles

def perspective_transform(img, src_points, width, height):
    """对图像进行透视变换"""
    dst_points = np.array([
        [0, 0],
        [width-1, 0],
        [width-1, height-1],
        [0, height-1]
    ], dtype="float32")
    matrix = cv2.getPerspectiveTransform(src_points, dst_points)
    warped = cv2.warpPerspective(img, matrix, (width, height))
    return warped, matrix

def inverse_perspective_point(point, transform_matrix):
    """对单个点进行透视逆变换"""
    inverse_matrix = cv2.invert(transform_matrix)[1]
    point_homogeneous = np.array([[[point[0], point[1]]]], dtype=np.float32)
    transformed_point = cv2.perspectiveTransform(point_homogeneous, inverse_matrix)
    return (int(transformed_point[0][0][0]), int(transformed_point[0][0][1]))

def draw_crosshair(img, x, y, size, color, thickness):
    """绘制十字标记"""
    x, y, size = int(x), int(y), int(size)
    cv2.line(img, (x - size, y), (x + size, y), color, thickness)
    cv2.line(img, (x, y - size), (x, y + size), color, thickness)

def calculate_and_print_error(cross_pos, rect_center):
    """计算并打印绿色十字与矩形中心的误差"""
    xerr = rect_center[0] - cross_pos[0]
    yerr = cross_pos[1] - rect_center[1] 
    print(f"xerr = {xerr}, yerr = {yerr}")
    data = "xerr={},yerr={}".format(xerr, yerr)
    serial0.write_str(data)
    return xerr, yerr

def draw_rectangles(img_bgr, rectangles, inverse_center_point=None):
    """在图像上绘制矩形、角点、中心、逆变换点，以及绿色十字"""
    colors = [(0,255,0),(255,0,0),(0,0,255),(255,255,0)]
    for i, rect in enumerate(rectangles):
        color = colors[i % len(colors)]
        corners = rect['corners'].astype(np.int32).reshape((-1,1,2))  # 修改这里
        cv2.polylines(img_bgr, [corners], True, color, 2)
        for j, corner in enumerate(rect['corners'].astype(np.int32)):  # 修改这里
            cv2.circle(img_bgr, tuple(corner), 5, color, -1)
            cv2.putText(img_bgr, str(j+1), (corner[0]+8, corner[1]-8),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)
        cx, cy = rect['center']
        cv2.circle(img_bgr, rect['center'], 3, (0,0,255), -1)
        cv2.circle(img_bgr, rect['center'], 5, (255,255,255), 1)
        cv2.putText(img_bgr, f"Rect{i+1} Center:{(cx,cy)}",
                    (10, 20 + i*20), cv2.FONT_HERSHEY_SIMPLEX, 0.4, color, 1)
    if inverse_center_point:
        cv2.circle(img_bgr, inverse_center_point, 1, (0,0,255), -1)
        cv2.putText(img_bgr, f"Inv Center:{inverse_center_point}",
                    (10, 200), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0,0,255), 1)
    # 最后绘制绿色十字
    draw_crosshair(img_bgr, cross_x, cross_y, cross_size, cross_color_green, cross_thickness)

# 全局保存透视矩阵和逆变换点
current_transform_matrix = None
current_inverse_center = None

while not app.need_exit():
    img = cam.read()

    if frame_count % process_every_n_frames == 0:
        img_bgr = image.image2cv(img, ensure_bgr=True, copy=False)
        gray = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY)

        rectangles = find_rectangles(gray)

        if rectangles and show_perspective:
            largest = rectangles[0]
            warped, matrix = perspective_transform(
                img_bgr, largest['corners'], screen_width, screen_height
            )
            current_transform_matrix = matrix
            inv_c = inverse_perspective_point(
                (perspective_center_x, perspective_center_y), matrix
            )
            current_inverse_center = inv_c

            # 计算并打印透视模式下的误差（透视中心与透视变换后图像中心的误差）
            calculate_and_print_error((perspective_center_x, perspective_center_y), (screen_width//2, screen_height//2))

            # 在透视图上绘制信息
            cv2.putText(warped, "Perspective View", (10,25),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0), 2)
            cv2.putText(warped, f"Area: {largest['area']:.0f}", (10,50),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0),1)
            cv2.putText(warped, f"Inv Center: {inv_c}", (10,75),
                        cv2.FONT_HERSHEY_SIMPLEX,0.4,(0,255,0),1)
            draw_crosshair(warped, perspective_center_x, perspective_center_y, 5, (255,255,255), 1)

            disp.show(image.cv2image(warped, copy=False))

        elif rectangles and not show_perspective:
            # 用最大矩形的周长计算绿色十字的 x 坐标
            perimeter = rectangles[0]['perimeter']
            #cross_x = int(-0.0481 * perimeter + 172.126)
            cross_x = int(-0.065 * perimeter + 171.5 - 1)
            # 计算并打印绿色十字与最大矩形中心的误差
            largest_rect_center = rectangles[0]['center']
            calculate_and_print_error((cross_x, cross_y), largest_rect_center)

            draw_rectangles(img_bgr, rectangles, current_inverse_center)
            disp.show(image.cv2image(img_bgr, copy=False))

        else:
            data = "null"
            serial0.write_str(data)
            print(f"send: {data}")
            # 无检测情况，保持逻辑不变
            if not show_perspective:
                draw_crosshair(img_bgr, cross_x, cross_y, cross_size, cross_color_green, cross_thickness)
                if current_inverse_center:
                    cv2.circle(img_bgr, current_inverse_center, 1, (0,0,255), -1)
                    cv2.putText(img_bgr, f"Last Inv Center:{current_inverse_center}",
                                (10,200), cv2.FONT_HERSHEY_SIMPLEX,0.4,(0,0,255),1)
                disp.show(image.cv2image(img_bgr, copy=False))
            else:
                blurred = cv2.GaussianBlur(gray,(5,5),0)
                edged = cv2.Canny(blurred,60,180)
                cv2.putText(edged, "No rectangles detected", (10,25),
                            cv2.FONT_HERSHEY_SIMPLEX,0.5,255,1)
                disp.show(image.cv2image(edged, copy=False))

    frame_count += 1
    fps = time.fps()
    print(f"time: {1000/fps:.02f}ms, fps: {fps:.02f}")